#ifdef  MPI_ENDBG
#define DBG(...)		printf( __VA_ARGS__ )
#else
#define DBG(...)
#endif

#ifdef  MPI_ENDBG_STATE
#define DBS(...)		printf( __VA_ARGS__ )
#else
#define DBS(...)
#endif

#include <limits.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <malloc.h>

#include "bitonic_interface_mpi.h"

#include <mpi.h>


#define SWAP(a, b)  \
			a ^= b; \
			b ^= a; \
			a ^= b

#define CMP_SWAP(a, b)     {\
			if(a > b) {     \
				SWAP(a, b); \
			}               \
		}


static inline void cmpi_send(data_t*, data_t*, long, enum cmpi_cmd, int);
static inline void cmpi_recv(data_t*, data_t*, long, int);
static void bitonic_merge_rec(data_t*, size_t);
static void bitonic_merge_single(data_t*, data_t*, size_t);

/* Debugging */
extern void print_array(data_t*, size_t);

static
void bitonic_merge_rec(data_t *data, size_t l) {
	if(l <= 1) {
		return;
	}

	size_t step = (l/2);
	size_t q = step;

	if(l%2 == 1) {
		if(data[ step ] < data[ (l-1) ]) {
			q++;
		}
		step++;
	}

	for(size_t i = 0; i < (l/2); i++) {
		CMP_SWAP(data[i], data[i+step]);
	}

	//print_array(data, l);
	DBG("[0] +(%ld), [%ld] +(%ld)\n", q, q, l-q);
	bitonic_merge_rec(data, q);
	bitonic_merge_rec(data+q, l-q);
}

static
void bitonic_merge_single(data_t *d1, data_t *d2, size_t l) {
	size_t i = 0;
	size_t j = 0;

	for(; i < l; i++, j++) {
		DBG(" [%d] cmp [%d]\n", d1[i], d2[j]);
		CMP_SWAP(d1[i], d2[j]);
	}
}

void bitonic_merge(data_t *data, size_t q, size_t l) {
	if(l == 1) {
		return;
	}
	size_t m = (l-1)/2;
	DBG(" q[%ld] l[%ld]\n", q, l);

	/* [0] -> [q], [q+1] -> [l-1] */
	if(q <= m) {
		size_t i = 0;
		size_t j = (2*q)+1;

		for(; i <= q; i++, j--) {
			DBG("  0:0  i[%ld] j[%ld]\n", i, j);
			CMP_SWAP(data[i], data[j]);
		}
	} else /* q > m */ {
		size_t i = q-((l-1)-q)+1;
		size_t j = (l-1);

		for(; j > q; i++, j--) {
			DBG("  1:1  i[%ld] j[%ld]\n", i, j);
			CMP_SWAP(data[i], data[j]);
		}
	}
	//print_array(data, l);
	DBG("[0] +(%ld), [%ld] +(%ld)\n", q+1, q+1, l-q-1);
	//bitonic_merge_rec(data, q+1);
	//bitonic_merge_rec(data+(q+1), l-q-1);
}

void cmpi_send(data_t *d1, data_t *d2, long l, enum cmpi_cmd c, int r) {
	DBS("Sending %ld data_t to %d .. ", l, r);
	MPI_Send(&c, 1, MPI_UINT32_T, r, CMPI_ALL, MPI_COMM_WORLD);
	MPI_Send(&l, 1, MPI_LONG, r, CMPI_ALL, MPI_COMM_WORLD);
	MPI_Send(d1, l,  CMPI_DATA_T, r, CMPI_ALL, MPI_COMM_WORLD);
	MPI_Send(d2, l,  CMPI_DATA_T, r, CMPI_ALL, MPI_COMM_WORLD);
	DBS("Done\n");
}

void cmpi_recv(data_t *d1, data_t *d2, long l, int r) {
	DBS("Receiving %ld data_t from %d .. ", l, r);
	//long l_t;
	MPI_Status status;
	/*MPI_Recv(&l_t, 1,  MPI_LONG, r, CMPI_ALL, MPI_COMM_WORLD, &status);
	if(l != l_t) {
		printf(" %d ERRRRRR %ld %ld\n", r, l, l_t);
	}*/
	MPI_Recv(d1, l,  CMPI_DATA_T, r, CMPI_ALL, MPI_COMM_WORLD, &status);
	MPI_Recv(d2, l,  CMPI_DATA_T, r, CMPI_ALL, MPI_COMM_WORLD, &status);
	DBS("Done\n");
}

void bitonic_merge_master(data_t *data, long l, int r_start, int r_end) {
	data_t *state_data[(r_end - r_start)];
	long state_l   [(r_end - r_start)];
	int r_total = (r_end - r_start);
	//int l_save = l;
	//data_t *data_save = data;

	state_data[0] = data;
	state_l[0] = l;

	for(int a = r_end; a > 0; a >>= 1) {
		DBG(" --\n");
		//print_array(data_save, l_save);
		for(int b = 0; b < r_total; b += a) {

			r_start = b;
			r_end = b+a;
			data = state_data[b];
			l = state_l[b];
			DBG(" r_s[%d] r_e[%d] l[%ld] b[%d]\n", r_start, r_end, l, b);

			int r_amount = (r_end - r_start);
			enum cmpi_cmd c = CMPI_DO_SINGLE;

			if(r_amount == 1) {
				DBS(" Reached sequential.\n");
				//c = CMPI_DO_REC;
				bitonic_merge_rec(data, l);
				continue;
			}
			int r_step = (l/2)/r_amount;
			int start = 0;

			size_t step = l/2;
			size_t q = step;

			if(l%2 == 1) {
				DBG(" odd (%ld)[%d] (%ld)[%d]\n", step, data[step], l-1, data[l-1]);
				if(data[step] < data[l-1]) {
					q++;
				}
				step++;
			}

			DBG(" l[%ld] r_s[%d] r_e[%d] r_step[%d]\n", l, r_start, r_end, r_step);

			if((l/2) % r_amount != 0) {
				DBG(" o\n");
				size_t rest = (l/2) % r_amount;
				//for(int i = r_start; i < (r_end-1); i++) {
				for(int i = (r_end-1); i > r_start; i--) {
					if(rest > 0) {
						rest--;
						//bitonic_merge_single(data+start, data+start+step, r_step+1);
						DBG("  p[%d] [%d] > [%d] u [%ld] > [%ld]\n", i, start, start+r_step+1, start+step, start+r_step+1+step);
						cmpi_send(data+start, data+start+step, r_step+1, c, i);
						start += r_step+1;
					} else {
						//bitonic_merge_single(data+start, data+start+step, r_step);
						DBG("  p[%d] [%d] > [%d] u [%ld] > [%ld]\n", i, start, start+r_step, start+step, start+r_step+step);
						cmpi_send(data+start, data+start+step, r_step, c, i);
						start += r_step;
					}
				}

				r_step = ((l/2)-start);
				if(c == CMPI_DO_SINGLE) {
					bitonic_merge_single(data+start, data+start+step, r_step);
				} else if(c == CMPI_DO_REC) {
					bitonic_merge_rec(data+start, r_step*2);
				} else {
					printf("Unknown command\n");
				}
				//cmpi_send(data+start, data+start+step, r_step, CMPI_DO_SINGLE, r_start);
				DBG("  p[%d] [%d] > [%d] u [%ld] > [%ld]\n", r_end-1, start, start+r_step, start+step, start+r_step+step);

				r_step = (l/2)/r_amount;
				rest = (l/2) % r_amount;
				start = 0;
				for(int i = (r_end-1); i > r_start; i--) {
					if(rest > 0) {
						rest--;
						//bitonic_merge_single(data+start, data+start+step, r_step+1);
						DBG("  p[%d] [%d] > [%d] u [%ld] > [%ld]\n", i, start, start+r_step+1, start+step, start+r_step+1+step);
						cmpi_recv(data+start, data+start+step, r_step+1, i);
						start += r_step+1;
					} else {
						//bitonic_merge_single(data+start, data+start+step, r_step);
						DBG("  p[%d] [%d] > [%d] u [%ld] > [%ld]\n", i, start, start+r_step, start+step, start+r_step+step);
						cmpi_recv(data+start, data+start+step, r_step, i);
						start += r_step;
					}
				}
			} else {
				//for(int i = r_start; i < r_end; i++) {
				for(int i = r_end-1; i > r_start; i--) {
					//bitonic_merge_single(data+start, data+start+step, r_step);
					DBG("  p[%d] [%d] > [%d] u [%ld] > [%ld]\n", i, start, start+r_step, start+step, start+r_step+step);
					cmpi_send(data+start, data+start+step, r_step, c, i);
					start += r_step;
				}
				DBS("-\n");
				if(c == CMPI_DO_SINGLE) {
					DBS("Running single\n");
					bitonic_merge_single(data+start, data+start+step, r_step);
				} else if(c == CMPI_DO_REC) {
					DBS("Running recursive\n");
					bitonic_merge_rec(data+start, r_step*2);
				} else {
					printf("Unknown command\n");
				}
				start = 0;
				for(int i = r_end-1; i > r_start; i--) {
					//bitonic_merge_single(data+start, data+start+step, r_step);
					DBG("  p[%d] [%d] > [%d] u [%ld] > [%ld]\n", i, start, start+r_step, start+step, start+r_step+step);
					cmpi_recv(data+start, data+start+step, r_step, i);
					start += r_step;
				}
			}
			state_data[b] = data;
			state_l[b] = q;

			state_data[b+a/2] = data+q;
			state_l[b+a/2] = l-q;
		}
	}
	enum cmpi_cmd c = CMPI_EXIT;
	DBS("Broadcasting halt\n");
	for(int i = 1; i < r_total; i++) {
		MPI_Send(&c, 1, MPI_UINT32_T, i, CMPI_ALL, MPI_COMM_WORLD);
	}
	//print_array(data_save, l_save);
}

void bitonic_merge_child(int r, int r_total) {
	long l;
	data_t *d1, *d2;
	enum cmpi_cmd c;
	MPI_Status status;

	DBS("  %d listening\n", r);
	MPI_Recv(&c, 1, MPI_UINT32_T, 0, CMPI_ALL, MPI_COMM_WORLD, &status);

	while(c != CMPI_EXIT) {
		MPI_Recv(&l, 1, MPI_LONG, 0, CMPI_ALL, MPI_COMM_WORLD, &status);

		d1 = malloc(l*sizeof(data_t)*2);
		d2 = d1+l;

		MPI_Recv(d1, l, CMPI_DATA_T, 0, CMPI_ALL, MPI_COMM_WORLD, &status);
		MPI_Recv(d2, l, CMPI_DATA_T, 0, CMPI_ALL, MPI_COMM_WORLD, &status);

		DBS("  %d received %d data_t\n", r, l);

		if(c == CMPI_DO_SINGLE) {
			DBS("  %d running single\n", r);
			bitonic_merge_single(d1, d2, l);
			DBS("  %d sending %d data_t to master .. ", r, l);
			//MPI_Send(&l, 1, MPI_LONG, 0, CMPI_ALL, MPI_COMM_WORLD);
			MPI_Send(d1, l, CMPI_DATA_T, 0, CMPI_ALL, MPI_COMM_WORLD);
			MPI_Send(d2, l, CMPI_DATA_T, 0, CMPI_ALL, MPI_COMM_WORLD);
			DBS("Done\n");
		} else if(c == CMPI_DO_REC) {
			DBS("  %d running recursive\n", r);
			bitonic_merge_rec(d1, l*2);
			DBS("  %d sending %d data_t to master .. ", r, l);
			//MPI_Send(&l, 1, MPI_LONG, 0, CMPI_ALL, MPI_COMM_WORLD);
			MPI_Send(d1, l, CMPI_DATA_T, 0, CMPI_ALL, MPI_COMM_WORLD);
			MPI_Send(d2, l, CMPI_DATA_T, 0, CMPI_ALL, MPI_COMM_WORLD);
			DBS("Done\n");
		} else {
			printf("Unkown command\n");
		}

		free(d1);
		DBS("  %d listening\n", r);
		MPI_Recv(&c, 1, MPI_UINT32_T, 0, CMPI_ALL, MPI_COMM_WORLD, &status);
	}
	DBS("  %d halting.\n", r);
}
